//
// Created by 19594 on 25-7-11.
//

#include "Motor.h"
#include "tim.h"
#include "gpio.h"
extern uint32_t sys_tick;
extern int Speed_left,Speed_right;
void Motor_Init(void) {
    HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);
    HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL);
    HAL_GPIO_WritePin(STBY_GPIO_Port,STBY_Pin,GPIO_PIN_SET);
}
void MotorA_Speed(uint8_t mode,uint16_t speed) {
    if (mode==1) {
        HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);
    }
    if (mode==2) {
        HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_SET);
    }
    __HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_1,speed);
}
void MotorB_Speed(uint8_t mode,uint16_t speed) {
    if (mode==1) {
        HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_RESET);
    }
    if (mode==2) {
        HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_SET);
    }
    __HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_2,speed);
}
int Get_Encoder(TIM_HandleTypeDef *htim) {
    int temp;
    temp=(short)__HAL_TIM_GET_COUNTER(htim);
    __HAL_TIM_SET_COUNTER(htim,0);
    return temp;
}
void Read_Speed() {
    if (uwTick-sys_tick<10)
        return ;
    sys_tick = uwTick;
    Speed_right=-Get_Encoder(&htim3);
    Speed_left=Get_Encoder(&htim1);
}